#!/usr/bin/env python3

import rospy
import serial
from std_msgs.msg import String

class SerialComNode:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('serial_com', anonymous=True)

        # 配置串口参数
        self.serial_port = '/dev/ttyUSB0'    # 串口设备名称
        self.baudrate = 115200               # 波特率
        self.serial = serial.Serial(self.serial_port, self.baudrate, timeout=1)

        # 订阅joy_ctrl话题
        self.sub = rospy.Subscriber('joy_ctrl', String, self.joy_callback)

    def joy_callback(self, joy_msg):
        # 从joy_ctrl话题接收到的消息
        data = joy_msg.data
        # data = "G1 X10 Y10 F10\n"

        # 发送到串口
        self.serial.write(data.encode())
        rospy.loginfo("Sent to serial: %s", data)

if __name__ == '__main__':
    try:
        serial_com_node = SerialComNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass